#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "zdyac/action/zdy_act.hpp"

using zdyac::action::ZdyAct;
using std::placeholders::_1;
using std::placeholders::_2;
using GoalHandleProgress = rclcpp_action::ServerGoalHandle<ZdyAct>;

class acserver : public rclcpp::Node {
public:
  explicit acserver(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
  : Node("acserver", options) {
    this->action_server = rclcpp_action::create_server<ZdyAct>(
        this->get_node_base_interface(),
        this->get_node_clock_interface(),
        this->get_node_logging_interface(),
        this->get_node_waitables_interface(),
        "get_tmp",
        std::bind(&acserver::handle_goal, this, _1, _2),
        std::bind(&acserver::handle_cancel, this, _1),
        std::bind(&acserver::handle_accepted, this, _1),
        rcl_action_server_get_default_options());
    RCLCPP_INFO(this->get_logger(), "服务端已创建");
  }

private:
  // 处理请求数据
  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const ZdyAct::Goal> goal) {
    (void)uuid;
    RCLCPP_INFO(this->get_logger(), "接收到动作客户端请求，初始温度为 %ld", goal->now);
    if (goal->now < 1) return rclcpp_action::GoalResponse::REJECT;
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleProgress> goal_handle) {
    (void)goal_handle;
    RCLCPP_INFO(this->get_logger(), "接到取消请求");
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void execute(const std::shared_ptr<GoalHandleProgress> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "开始");
    rclcpp::Rate loop_rate(10.0);
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<ZdyAct::Feedback>();
    auto result = std::make_shared<ZdyAct::Result>();
    int64_t sum = goal->now;
    int i=0;
    while(sum<100) {
      if(sum<50)sum = sum+i*i;
      else sum=sum+i;
      i++;
      if (goal_handle->is_canceling()) {
        result->final = sum;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "任务取消");
        return;
      }
      if(sum>=100)sum=100;
      feedback->ing = sum;
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "%ld", feedback->ing);
      loop_rate.sleep();
    }
    if (rclcpp::ok()) {
      result->final = sum;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "任务完成");
    }
  }

  void handle_accepted(const std::shared_ptr<GoalHandleProgress> goal_handle) {
    std::thread{std::bind(&acserver::execute, this, _1), goal_handle}.detach();
  }

  rclcpp_action::Server<ZdyAct>::SharedPtr action_server;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<acserver>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}